#include<stdio.h>
#include "Arcroll.h"
#include<Mesh_IO/Mesh_IO.h>

#include "directories_and_files.h"
#include "test_vgicp.h"
#define quote lib_cell_quote


void static inline binary_read_one_line(FILE* infile,char* str)
{
    memset(str,0,sizeof(char)*300);
    char temp='\0';

    fread(&temp,sizeof(char),1,infile);
    int i=0;
    while( temp!='\n' )
    {
        //printf("%d %c\n",i,temp);
        str[i]=temp;
        i++;
        if(feof(infile))
        {
            return ;
        }
        fread(&temp,sizeof(char),1,infile);
    } 
    if(str[i-1]=='\r')
    {
        str[i-1]='\0';
    }
}



Node* read_local_txt(const char* filename,int *num  )
{
    *num=0;
#ifdef _WIN32
    FILE *infile;
    fopen_s(&infile,filename,"r");
#else
    FILE *infile=fopen(filename,"r");
#endif
    if(!infile)
    {
        printf("can't open this file %s\n",filename);
    }
    fseek(infile,0,SEEK_SET); 
    char str[300]={0};
    Node* re=NULL;
    while(!feof(infile))
    {
        memset(str,0,sizeof(char)*300);
        fgets(str,sizeof(char)*300,infile);
        int n=strlen(str)+2;
        char* temp=(char*)malloc(sizeof(char)*(n) );
        memset(temp,0,sizeof(char)*n);
        strcpy(temp,str);
        re=node_pushback(re,temp);

        *num=*num+1;
    }
 

    return node_reverse( re);
}

float* get_weizi_from_node(Node* n,int num,char** files)
{
    float* re=(float*)malloc(sizeof(float)*4*num);

    float temp[3]={0};
    int i=0;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        char *str=(char*)(nit->value);
        files[i]=(char*)malloc(sizeof(char)*200);
        memset(files[i],0,sizeof(char)*200);


        sscanf(str, "%s %f %f %f %f %f %f\n",files[i],&(re[i*4+0]),&(re[i*4+1]),&(re[i*4+2]), &(temp[1]),&(temp[2])  , &(re[i*4+3])   ); 
        
        int len=strlen(files[i]);
        files[i][len]='.';files[i][len+1]='p';files[i][len+2]='c';files[i][len+3]='d';

        for(int j=10;j<len+4;j++)
        {

            files[i][j]=files[i][j+1];
        } 

        printf("%s\n",files[i]);
        
        //printf("%f %f %f %f \n",re[i*4+0],re[i*4+1],re[i*4+2],re[i*4+3]);

        i++;
    } 
    return re;

}

float*  my_read_pcd (const char  *filename,int *num)
{
    printf("begin read pcd\n");
    int result;
#ifdef _WIN32
    FILE *infile;
    fopen_s(&infile,filename,"rb");
#else
    FILE *infile=fopen(filename,"rb");
#endif
    if(!infile)
    {
        printf("can't open this file %s\n",filename);
        return NULL;
    }
    fseek(infile,0,SEEK_SET); 

    char str[300]={0},str1[300]={0},temp_str[300]={0};
    char str2[300]={0},str3[300]={0};

    // binary_read_one_line(infile,str);
    // printf("%s\n",str);

    *num=0;
    while(!feof(infile)&&!(strcmp(str,"DATA ascii")==0|| strcmp(str,"DATA binary")==0  ))
    {
        binary_read_one_line(infile,str);
        //printf("%s\n",str);

        if(str[0]=='#')
        {
            continue;
        }

        if(str[0]=='P'&&str[1]=='O'&&str[2]=='I'&&str[3]=='N'&&str[4]=='T'&&str[5]=='S')
        {
            sscanf(&(str[7]), "%d", num );
        }
        else if(str[0]=='F'&&str[1]=='I'&&str[2]=='E'&&str[3]=='L'&&str[4]=='D'&&str[5]=='S')
        {
            strcpy(str1,&(str[7])); 

        }
        else if( str[0]=='S'&&str[1]=='I'&&str[2]=='Z'&&str[3]=='E' )
        {
            strcpy(str2,&(str[5])); 
        }
        else if(str[0]=='C'&&str[1]=='O'&&str[2]=='U'&&str[3]=='N'&&str[4]=='T'  )
        {
            strcpy(str3,&(str[6]) );

        }
        // else if ()
        // {

        // }
    }

    //printf("%s %s\n",str1,str2);
    int n_str1,n_str2,n_str3,ns[10]={0},ns1[10]={0};
    char** str11= split(str1,' ',&n_str1);
    char** str22= split(str2,' ',&n_str2);
    char** str33=split(str3,' ',&n_str3); 
    for(int i=0;i<n_str2;i++)
    {
        sscanf(str22[i],"%d",&(ns[i]) );
        sscanf(str33[i],"%d",&(ns1[i]) );
        //printf("%d \n",ns[i]);
    }

    if(feof(infile))
    {
        return NULL;
    }
    float * re=(float*)malloc(sizeof(float)*3*(*num));
    //printf("num: %d\n",*num);
    float p[3]={0};double dp[3]={0};
    if(strcmp(str,"DATA ascii")==0 )
    {

        for(int i=0;i<*num;i++)
        {
            for(int j=0;j<n_str1;j++)
            {
                if( strcmp(str11[j],"x" )==0 )
                {
                    fscanf(infile,"%f",&(p[0])); 
                }  
                else if(strcmp(str11[j],"y")==0)
                {
                    fscanf(infile,"%f",&(p[1]));
                }
                else if(strcmp(str11[j],"z")==0)
                {
                    fscanf(infile,"%f",&(p[2]));
                }
                else 
                {
                    fscanf(infile,"%s", temp_str);
                    memset(temp_str,0,sizeof(char)*300);

                }
               

            } 
            //dp[0]=p[0];dp[1]=p[1];dp[2]=p[2];
            memmove(&(re[i*3]),p,sizeof(float)*3 );
            //printf("%f %f %f\n",p[0],p[1],p[2]);
        }
    }
    else if(strcmp(str,"DATA binary")==0)
    {

        for(int i=0;i<*num;i++)
        {

            for(int j=0;j<n_str1;j++)
            {
                if( strcmp(str11[j],"x" )==0 )
                {
                    fread(&(p[0]),sizeof(float),1,infile);
                }  
                else if(strcmp(str11[j],"y")==0)
                {
                    fread(&(p[1]),sizeof(float),1,infile);
                }
                else if(strcmp(str11[j],"z")==0)
                {
                    fread(&(p[2]),sizeof(float),1,infile);
                }
                else 
                {
                    fread(temp_str,ns[j] ,ns1[j],infile); 
                    memset(temp_str,0,sizeof(char)*300);

                }
            } 
            memmove(&(re[i*3]),p,sizeof(float)*3 );
            //printf("%f %f %f\n",p[0],p[1],p[2]);
            
            //dp[0]=p[0];dp[1]=p[1];dp[2]=p[2];
            
        }
    }

    for(int i=0;i<n_str1;i++)
    {
        free(str11[i]);
    }   
    for(int i=0;i<n_str2;i++)
    {
        free(str22[i]);
    }
    for(int i=0;i<n_str3;i++)
    {
        free(str33[i]);
    }
    free(str11);free(str22);free(str33);


    fclose(infile);

    return re;
}
typedef struct SLAM_ANI {
    int is_play; 
    float begin_time,run_time; 

    int n,length;

    //Node* files_info;
    float *xyz_an;
    char **pcd_files;
    Viewer_Something* vs,*vs1;
    Viewer_Opengl_Interpreter* voi;
    void* prop; 

    //Node* info;

}SLAM_ANI;






static inline void slam_ani_init(SLAM_ANI* sa )
{
    sa->is_play=0;
    sa->begin_time=0;sa->run_time=0;
    sa->n=0;
    sa->prop=NULL;
    sa->xyz_an=NULL;
    sa->pcd_files=NULL;
    //memset(sa->xyz_an,0,sizeof(float)*4);
    sa->vs=NULL;sa->vs1=NULL;
    sa->voi=NULL;
    //sa->info=NULL;
    sa->length=0;

}
#define MY_PI 3.1415926535
void sa_animation_callback (Viewer_Intera* vi)
{
   Interactor_GlobalInfo* g_info=vi->g_info; 
   SLAM_ANI* sa=(SLAM_ANI*)(vi->representation);


    if(sa->is_play==0)
    {
        return ;
    } 


    sa->run_time+= (g_info->run_time- sa->begin_time);
    sa->begin_time=g_info->run_time;
      
    if(sa->run_time>sa->n*0.8 )
    {


        sa->n++;
        printf("once %f %f %f %f\n",sa->xyz_an[sa->n*4+0],sa->xyz_an[sa->n*4+1], sa->xyz_an[sa->n*4+2],sa->xyz_an[sa->n*4+3]);

        Viewer_Points* vp=(Viewer_Points*)(sa->vs->evolution);
        
        Viewer_Points* vp1=(Viewer_Points*)(sa->vs1->evolution);

        char str[300]="/home/libo/Documents/data/pcd/zzzl_pcd/";
        strcat(str,sa->pcd_files[sa->n+3]);

        int num=0;
        float* vpdata=    my_read_pcd(str,&num); 

        if(vpdata==NULL)
        {
            return;
        }

        free( vp1->Data);
 

        vp1->Data= vpdata; 

        
        printf("num :%d\n",num);

        vp1->Data_rows=num ;  
        vp1->set_color(vp1,0.2,0.9,0.2,1.0);

        vp->mat->copy_data(vp->mat,vp1->mat);


        LB_Matrix* mat= (LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(mat,4,4);

        float* data=(float*)(mat->data);
        //float theta=sa->xyz_an[sa->n*4+3]*MY_PI/180.0 ;  
        float theta=sa->xyz_an[sa->n*4+3];
        data[0*4+0]=cos(theta); data[0*4+1]=-sin(theta); 
        data[1*4+0]=sin(theta); data[1*4+1]=cos(theta); 


        data[0*4+3]=sa->xyz_an[sa->n*4+0];
        data[1*4+3]=sa->xyz_an[sa->n*4+1];
        data[2*4+3]=sa->xyz_an[sa->n*4+2];

        LB_Matrix* mati=mat->inverse(mat);

        vp->mat->mult(vp->mat,mati);

//**************

       


     



//***********************
        lb_matrix_free(mati);


        lb_matrix_free(mat);

        sa->voi->update_data(sa->voi);
    }

}
void sa_key_callback(Viewer_Intera*vi)
{
    Interactor_GlobalInfo* g_info=vi->g_info; 
    SLAM_ANI* sa=(SLAM_ANI*)(vi->representation);
    if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_P&&g_info->key_mods==0)
    {        
        if(sa->is_play==0)
        {
            sa->begin_time=g_info->run_time;
            sa->is_play=1;
        } 
        else
        {
            sa->is_play=0;
        }
    } 
    else if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_Z&&g_info->key_mods==0)
    {
        Viewer_Points* vp=(Viewer_Points*)(sa->vs->evolution);
        float*mat_data= (float*)(sa->vs->prop);
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                ((float*)(vp->mat->data))[i*4+j]=mat_data[j*4+i];
            }
        }
    }
    else if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_N&&g_info->key_mods==0)
    {
        Viewer_Points* vp=(Viewer_Points*)(sa->vs->evolution);
        vp->mat->identity(vp->mat);

    }
}

//get_matrix_from_rotation
#include<Math/LB_Quaternions.h>

double * get_guess_mat()
{
    double * re=(double*)malloc(sizeof(double)*16);
    re[0*4+3]=139.251+3.8; re[1*4+3]=-108; re[2*4+3]=3.766797;

    
    double axis[3]={0.0 ,0.0 ,1.0};
    double* guess_mat= get_matrix_from_rotation(-3.094451,axis );
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            re[i*4+j]=guess_mat[i*3+j];

        }
    }


    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            printf("%f ",re[i*4+j]);

        }
        printf("\n");
    }

    free(guess_mat);

    return re;
}

int main()
{
   
    //cuda_vgicp_align
    //double* guess_mat= get_guess_mat();
    float* mat_data=cuda_ndt_align("./1658566028027018.pcd","./1658566028227288.pcd",NULL);
    


    //printf("%f %f %f %f %f %f %f %f %f %f\n",re[0])
    
    for(int i=0;i<16;i++)
    {
        printf("%f ",mat_data[i]);
    }
    printf("\n");


	Viewer_World_Manager vwm;
	viewer_world_manager_init(&vwm);
	Viewer_World* vw=vwm.create_world(&vwm,"中原动力位姿标注");


	Viewer_Opengl_Interpreter voi;
	Viewer_Arcroll* va= add_default_somethings(vw);


    SLAM_ANI sa;
    slam_ani_init(&sa);
    sa.voi=&voi;
	int num=0;

    
//

    float* data=my_read_pcd("./1658566028027018.pcd",&num);	
	// float* data=my_read_pcd("/home/libo/Documents/data/zzzl.pcd",&num);	
    Node*n=vw->create_something(vw,"Points");

	auto vs=(Viewer_Something*)(n->value);
	auto vp=(Viewer_Points*)(vs->evolution);
    vs->prop= mat_data;
	vp->Data_rows=num;
	// for(int i=0;i<4;i++)
    // {
    //     for(int j=0;j<4;j++)
    //     {
    //         ((float*)(vp->mat->data))[i*4+j]=mat_data[j*4+i];
    //         printf("%d ",j*4+i);
    //     }
    // }

    vp->mat->print_self(vp->mat);

	vp->Data=data;
	vp->set_color(vp,0.9,0.3,0.7,1.0);
	vp->pointsize=9.0;
	free_node(n);	
    sa.vs=vs;
    
   
    data=my_read_pcd("./1658566028227288.pcd",&num); 

    n=vw->create_something(vw,"Points");

    vs=(Viewer_Something*)(n->value);
    vp=(Viewer_Points*)(vs->evolution);
    //sa.vs1=vs;
    vp->Data_rows=num;
                    
    vp->Data=data;
    vp->set_color(vp,0.2,0.9,0.2,1.0);
    
    vp->pointsize=8.0;
    free_node(n);


    
    // Node* info=read_local_txt("./localization.txt",&num );
    // printf("txt:%d\n",num);
    // sa.length=num;
    // sa.pcd_files=(char**)malloc(sizeof(char*)*num);
    // sa.xyz_an= get_weizi_from_node(info,num,sa.pcd_files);
  




    free_node_value(info);
    free_node(info);

    // char **files=search_dir_files("/home/libo/Documents/data/pcd/zzzl_pcd",&num);
    // printf("%s %s %s\n",files[0],files[1],files[2]);
    // sa.pcd_files=files;






    n=vw->create_something(vw,"Intera");

    vs=(Viewer_Something*)(n->value);

    Viewer_Intera*vi=(Viewer_Intera*)(vs->evolution);
    
    vi->representation=&sa;
    
    //vi->cursor_position_callback=patp_move_workpiece_cursor_callback;
 //    //vi->scroll_callback=viewer_Arcroll_scroll_callback;
 //    //vi->mouse_button_callback=viewer_Arcroll_mouse_button_callback;


//******************* bei zhu shi ***********************

    vi->key_callback=sa_key_callback; 

    vi->animation=sa_animation_callback;

    //vi->animation=sa_animation_callback ;

 //************************ bei zhu shi ********************************


    //vi->char_callback=patp_char_callback;
 //    vi->prop=patp;

    free_node(n); 


	viewer_opengl_interpreter_initn(&voi,&vwm);
	voi.interpreter(&voi);


	viewer_opengl_interpreter_free(&voi);
	viewer_world_manager_free(&vwm); 	


	printf("endnnnnnn\n");
	return 0;
}
